#include "ExpendedTree.h"

ExpendedTree::ExpendedTree(Point_d start_point, Oracle* oracle, MultiRobotsSampler* multi_robots_sampler):
_start_point(start_point),
_oracle(oracle),
_multi_robots_sampler(multi_robots_sampler)
{
	_kd_tree = new Kd_tree_d<Kernel_d>();
	_tree = new Graph<int>(0,true);

	_kd_tree->insert(start_point);
	_tree->add_vertex(_all_tree_points.size());
	_all_tree_points.push_back(start_point);
}

int ExpendedTree::get_point_index(Point_d p)
{
	int i = 0;

	for (vector<Point_d>::iterator all_tree_points_it = _all_tree_points.begin();
		all_tree_points_it != _all_tree_points.end();
		all_tree_points_it++)
	{
		if (*all_tree_points_it == p)
		{
			return i;
		}

		i++;
	}

	return -1;
}

void ExpendedTree::expand(int num_of_iterations)
{
	for (int i = 0; i < num_of_iterations; ++i)
	{
		Point_d q_rand = _multi_robots_sampler->generate_sample_point_d();

		vector<Point_d> k_nearest_neighbors;

		_kd_tree->k_nearest_neighbors(q_rand,
										1,
										std::back_inserter(k_nearest_neighbors));

		Point_d q_near = k_nearest_neighbors[0];
		int q_near_index = get_point_index(q_near);

		vector<Point_d> legal_neighbors = _oracle->get_legal_neighbors(q_near);

		if (legal_neighbors.size() == 0)
		{
			continue;
		}

		Kernel_d::Vector_d main_vector = q_rand - q_near;

		Point_d min_angle_point = legal_neighbors[0];
		Kernel_d::Vector_d current_vactor = min_angle_point - q_near;

		double vector_mult = main_vector * current_vactor;

		double min_angle = acos(vector_mult /
			(sqrt(main_vector.squared_length()) * sqrt(current_vactor.squared_length()))) * 
			180.0 / PI;

		for (uint i = 1; i < legal_neighbors.size(); ++i)
		{
			current_vactor = legal_neighbors[i] - q_rand;
			vector_mult = main_vector * current_vactor;
			double current_angle = acos(vector_mult /
				(sqrt(main_vector.squared_length()) * sqrt(current_vactor.squared_length()))) * 
				180.0 / PI;

			if (current_angle < min_angle)
			{
				min_angle_point = legal_neighbors[i];
				min_angle = current_angle;
			}
		}

		if (-1 != get_point_index(min_angle_point))
		{
			continue;
		}

		int min_angle_point_index = _all_tree_points.size();

		_all_tree_points.push_back(min_angle_point);
		_kd_tree->insert(min_angle_point);
		_tree->add_vertex(min_angle_point_index);
		_tree->add_edge(q_near_index, min_angle_point_index);
	}
}
